﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace QuMianChaZhi
{
    class InterPolationKriging
    {

        /// <summary>
        /// 数组相关的操作
        /// </summary>
        /// 

        public List<double> Rep(int n, double orign)
        {
            List<double> rets = new List<double>(n);
            for(int i = 0; i < n; i++)
            {
                rets.Add(orign);
            }

            return rets;            
        }

        public List<int> Rep(int n, int orign)
        {
            List<int> rets = new List<int>(n);
            for (int i = 0; i < n; i++)
            {
                rets.Add(orign);
            }

            return rets;
        }

        public bool IsPip(int[][] inputArray,int x,int y)
        {
            bool c = false;

            int j = inputArray.Length - 1;

            for(int i = 0;i < inputArray.Length; j = i++)
            {
                if (((inputArray[i][1] > y) != (inputArray[j][1] > y)) &&
            (x < (inputArray[j][0] - inputArray[i][0]) * (y - inputArray[i][1]) / (inputArray[j][1] - inputArray[i][1]) + inputArray[i][0]))
                {

                    c = !c;
                }
            }


            return c;
        }
        public class DistanceObj
        {
            public double distance { get; set; }

            public double t { get; set; }
        }

        public class DistanceComparer : IComparer<DistanceObj>
        {
            public int Compare(DistanceObj x, DistanceObj y)
            {
                return (int)(x.distance - y.distance);
            }
        }

        //kriging

        // Matrix algebra矩阵代数
        List<double> kriging_matrix_diag(double c, int n)
        {
            //新建一个n*n的矩阵
            List<double> Z = Rep(n * n, 0.0);
            //循环赋值c给Z矩阵的每一元素
            for (int i = 0; i <n; i++)
            {
                Z[i * n + i] = c;
            }
            return Z;
        }

        //将这个矩阵变为转置阵，也就是将元素颠倒顺序
        List<double> kriging_matrix_transpose(List<double> X, int n, int m)
        {
            List<double> Z = Rep(m * n,0.0);
            for(int i = 0; i < n; i++)
            {
                for(int j = 0; j < m; j++)
                {
                    Z[j * n + i] = X[i * m + j];
                }
            }
            return Z;
        }

        //再次改变数值，把数组的每一个值扩大C
        void kriging_matrix_scale(ref List<double> X, double c, int n, int m)
        {
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < m; j++)
                {
                    X[i * m + j] *= c;
                }
            }
        }

        //数组相加
        List<double> kriging_matrix_add(List<double> X, List<double> Y, int n, int m)
        {

            //新建一个m*n的矩阵Z
            List<double> Z =  Rep(m * n, 0.0);
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < m; j++)
                {
                    //将X和Y矩阵相加合并成一个矩阵
                    Z[i * m + j] = X[i * m + j] + Y[i * m + j];
                }
            }
            //返回一个Z矩阵
            return Z;
        }

        // Naive matrix multiplication
        //简单的矩阵乘法，矩阵和矩阵的乘法
        //也就是前一个矩阵中的行乘以后一个矩阵中的列
        List<double>  kriging_matrix_multiply(List<double> X, List<double> Y, int n, int m, int p)
        {
            //新建一个m*n的矩阵Z
            List<double> Z =  Rep(p * n, 0.0);

            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < p; j++)
                {
                    Z[i * p + j] = 0;
                    for (int k = 0; k < m; k++)
                    {
                        Z[i * p + j] += X[i * m + k] * Y[k * p + j];
                    }
                }
            }
            return Z;
        }

        // Cholesky decomposition
        //柯列斯基分解，这是一种将正定矩阵分解为上三角矩阵和下三角矩阵的方法，
        //在优化矩阵计算的时候会用到的一种技巧
        //也就是，下面左边为下三角，右边为上三角
        //100000 123456
        //120000 023456
        //123000 003456
        //123400 000456
        //123450 000056
        //123456 000006
        bool kriging_matrix_chol(ref List<double> X, int n)
        {
            //lfchen : 这样创建不出来一个长度为n的容器
            // List<double> p = new List<double>(n);

            List<double> p = Rep(n,0.0);

            for (int i = 0; i < n; i++)
            {
                p[i] = X[i * n + i];
            }                
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < i; j++)
                {
                    p[i] -= X[i * n + j] * X[i * n + j];
                }

                if (p[i] <= 0) 
                    return false;

                p[i] = Math.Sqrt(p[i]);
                for (int j = i + 1; j < n; j++)
                {

                    for (int k = 0; k < i; k++)
                    {
                        X[j * n + i] -= X[j * n + k] * X[i * n + k];
                    }

                    X[j * n + i] /= p[i];
                }
            }
            for (int i = 0; i < n; i++) 
            {
                X[i * n + i] = p[i]; 
            }
            return true;
        }

        // Inversion of cholesky decomposition
        //用斯基分解求矩阵的逆

        void kriging_matrix_chol2inv (ref List<double> X, int n)
        {
            for (int i = 0; i < n; i++)
            {

                X[i * n + i] = 1 / X[i * n + i];
                for (int j = i + 1; j < n; j++)
                {

                    double sum = 0;
                    for (int k = i; k < j; k++)
                    {
                        sum -= X[j * n + k] * X[k * n + i];
                    }
                    X[j * n + i] = sum / X[j * n + j];
                }
            }
            for (int i = 0; i < n; i++)
            {
                for (int j = i + 1; j < n; j++)
                {
                    X[i * n + j] = 0;
                }
            }

            for (int i = 0; i < n; i++)
            {

                X[i * n + i] *= X[i * n + i];
                for (int k = i + 1; k < n; k++)
                {
                    X[i * n + i] += X[k * n + i] * X[k * n + i];
                }
                for (int j = i + 1; j < n; j++)
                {
                    for (int k = j; k < n; k++)
                    {
                        X[i * n + j] += X[k * n + i] * X[k * n + j];
                    }
                }
            }
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < i; j++)
                {
                    X[i * n + j] = X[j * n + i];
                }
            }

        }

        // Inversion via gauss-jordan elimination
        //用高斯-约当消去法求逆，它的速度不是最快的，但是它非常稳定
        //如果A是求解矩阵，那么求A的逆矩阵则为
        //用A矩阵右边乘以单位矩阵I（与A同行同列值为1的单位矩阵）
        //公式为A*I=I*B，(等号右边要同时变化)，也就是一个矩阵右边乘以单位矩阵化为，
        //左边单位矩阵乘以B，则B就是A矩阵的逆
        bool kriging_matrix_solve(ref List<double> X, int n)
        {

            int m = n;
            //var B = new List<double>(n * n);
            //var indxc = new List<int>(n);
            //var indxr = new List<int>(n);
            //var ipiv = new List<int>(n);

            List<double> B = Rep(n * n,0.0);
            List<int> indxc = Rep(n,0);
            List<int> indxr = Rep(n, 0);
            List<int> ipiv = Rep(n, 0);



            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    if (i == j)
                    {
                        B[i * n + j] = 1;
                    }
                    else
                    {
                        B[i * n + j] = 0;
                    }
                }
            }

            int icol = 0;
            int irow = 0;
            for (int i = 0; i < n; i++)
            {
                double big = 0;
                for (int j = 0; j < n; j++)
                {
                    if (ipiv[j] != 1)
                    {
                        for (int k = 0; k < n; k++)
                        {
                            if (ipiv[k] == 0)
                            {
                                if (Math.Abs(X[j * n + k]) >= big)
                                {
                                    big = Math.Abs(X[j * n + k]);
                                    irow = j;
                                    icol = k;
                                }
                            }
                        }
                    }
                }
                ++(ipiv[icol]);

                if (irow != icol)
                {

                    for (int l = 0; l < n; l++)
                    {

                        double temp = X[irow * n + l];
                        X[irow * n + l] = X[icol * n + l];
                        X[icol * n + l] = temp;
                    }
                    for (int l = 0; l < m; l++)
                    {

                        double temp = B[irow * n + l];
                        B[irow * n + l] = B[icol * n + l];
                        B[icol * n + l] = temp;
                    }
                }
                indxr[i] = irow;
                indxc[i] = icol;

                if (X[icol * n + icol] == 0)
                {
                    return false; // Singular
                }

                double pivinv = 1 / X[icol * n + icol];
                X[icol * n + icol] = 1;
                for (int l = 0; l < n; l++)
                {
                    X[icol * n + l] *= pivinv;
                }
                for (int l = 0; l < m; l++)
                {
                    B[icol * n + l] *= pivinv;
                }

                for (int ll = 0; ll < n; ll++)
                {

                    if (ll != icol)
                    {

                        double dum = X[ll * n + icol];
                        X[ll * n + icol] = 0;
                        for (int l = 0; l < n; l++)
                        {
                            X[ll * n + l] -= X[icol * n + l] * dum;
                        }
                        for (int l = 0; l < m; l++)
                        {
                            B[ll * n + l] -= B[icol * n + l] * dum;
                        }
                    }
                }
            }
            for (int l = (n - 1); l >= 0; l--)
            {
                if (indxr[l] != indxc[l])
                {
                    for (int k = 0; k < n; k++)
                    {
                        double temp = X[k * n + indxr[l]];
                        X[k * n + indxr[l]] = X[k * n + indxc[l]];
                        X[k * n + indxc[l]] = temp;
                    }
                }
            }
            return true;
        }

        public class Variogram
        {
            public List<double> t { get; set; }

            public List<double> x { get; set; }

            public List<double> y { get; set; }

            public List<double> K { get; set; }

            public List<double> M { get; set; }

            public double nugget { get; set; }

            public double range { get; set; }


            public double sill { get; set; }

            public double A { get; set; }

            public int n { get; set; }

            //最后一个double为返回值类型
            public Func<double, double, double, double, double,double> model = null;

        }

        // Variogram models
        //变差函数模型
        //变差函数高斯
        private double kriging_variogram_gaussian(double h,double nugget,double range,double sill,double A)
        {
            return nugget + ((sill - nugget) / range) *
            (1.0 - Math.Exp(-(1.0 / A) * Math.Pow(h / range, 2)));
        }

        //变差函数指数
        private double kriging_variogram_exponential(double h, double nugget, double range, double sill, double A)
        {
            return nugget + ((sill - nugget) / range) *
            (1.0 - Math.Exp(-(1.0 / A) * (h / range)));
        }

        //变差函数的球形
        private double kriging_variogram_spherical(double h, double nugget, double range, double sill, double A)
        {
            if (h > range) return nugget + (sill - nugget) / range;
            return nugget + ((sill - nugget) / range) *
                (1.5 * (h / range) - 0.5 * Math.Pow(h / range, 3));
        }
        public Variogram train(List<double> t,List<double> x, List<double> y, string model,double sigma2,double alpha)
        {
            Variogram variogram = new Variogram()
            {
                t = t,
                x = x,
                y = y,
                nugget = 0.0,
                range = 0.0,
                sill = 0.0,
                A = 1.0/3.0,
                n = 0
            };

            switch (model)
            {

                case "gaussian":
                    variogram.model = kriging_variogram_gaussian;
                    break;
                case "exponential":
                    variogram.model = kriging_variogram_exponential;
                    break;
                case "spherical":
                    variogram.model = kriging_variogram_spherical;
                    break;
            };

            // Lag distance/semivariance
            // 滞后距离/半方差
            int len, n;
             len = 0;
            n = t.Count;

            int distanceH = (n * n - n) / 2;

            //计算任意两个点之间的所有距离
            List<DistanceObj> distance = new List<DistanceObj>(distanceH);
            for(int i = 0; i < n; i++)
            {
                for(int j = 0; j < i; j++)
                {
                    DistanceObj disObj = new DistanceObj();
                    disObj.distance = Math.Pow(Math.Pow(x[i] - x[j], 2) + Math.Pow(y[i] - y[j], 2), 0.5);
                    disObj.t = Math.Abs(t[i] - t[j]);

                    distance.Add(disObj);
                }
            }

            distance.Sort(new DistanceComparer());

            //两个点之间最远的距离
            variogram.range = distance[distanceH - 1].distance;

            int NLength = (n * n - n) / 2;


            // 第一步
            // Bin lag distance
            //本滞后距离
            int lags = NLength > 30 ? 30 : NLength;

            double tolerance = variogram.range / lags;
            List<double> lag = Rep(lags, 0.0);
            List<double> semi = Rep(lags, 0.0);

            if(lags < 30)
            {
                for(len = 0; len < lags; len++)
                {
                    lag[len] = distance[len].distance;
                    semi[len] = distance[len].t;
                }
            }
            else
            {
                len = 0;
                for (int i=0,j=0; i < lags && j < NLength; i++)
                {
                    int k = 0;

                    while(distance[j].distance <= ((i + 1) * tolerance))
                    {
                        lag[len] = lag[len] + distance[j].distance;
                        semi[len] = semi[len]  + distance[j].t;
                        j++;
                        k++;

                        if(j >= NLength)
                        {
                            break;
                        }
                    }

                    if( k > 0)
                    {
                        lag[len] = lag[len] / k;
                        semi[len] = semi[len] / k;

                        len++;
                    }
                }

                // Error: Not enough points错误:分数不够
                if (len < 2)
                {
                    return variogram;
                }
            }

            //第二步
            // Feature transformation功能转换
            n = len;
            variogram.range = lag[n - 1] - lag[0];

            List<double> X = Rep(2 * n, 1.0);
            List<double> Y = Rep(n, 0.0);
            var A = variogram.A;

            for(int i = 0;i< n; i++)
            {
                switch (model)
                {

                    case "gaussian":
                        X[i * 2 + 1] = 1.0 - Math.Exp(-(1.0 / A) * Math.Pow(lag[i] / variogram.range, 2));
                        break;
                    case "exponential":
                        X[i * 2 + 1] = 1.0 - Math.Exp(-(1.0 / A) * lag[i] / variogram.range);
                        break;
                    case "spherical":
                        X[i * 2 + 1] = 1.5 * (lag[i] / variogram.range) - 0.5 * Math.Pow(lag[i] / variogram.range, 3);
                        break;
                };
                Y[i] = semi[i];
            }

            //第三步
            // Least squares最小平方

            List<double> Xt = kriging_matrix_transpose(X, n, 2);
            List<double> Z = kriging_matrix_multiply(Xt, X, 2, n, 2);

            List<double> Mt = kriging_matrix_diag(1 / alpha, 2);
            Z = kriging_matrix_add(Z, Mt, 2, 2);

            List<double> cloneZ = new List<double>(Z);

            bool isChol = kriging_matrix_chol(ref Z, 2);
            if(isChol)
            {
                kriging_matrix_chol2inv(ref Z, 2);
            }
            else
            {
                kriging_matrix_solve(ref cloneZ, 2);
                Z = cloneZ;
            }
            List<double> wT = kriging_matrix_multiply(Z, Xt, 2, 2, n);
            List<double> W = kriging_matrix_multiply(wT, Y, 2, n, 1);

            // Variogram parameters变差函数参数
            variogram.nugget = W[0];
            variogram.sill = W[1] * variogram.range + variogram.nugget;
            variogram.n = x.Count;


            //第四步
            // Gram matrix with prior有先验Gram矩阵

            n = x.Count;
            List<double> K = Rep(n * n,0.0);
            for(int i = 0; i < n; i++)
            {
                for(int j = 0; j < i; j++)
                {
                    K[i * n + j] = variogram.model(Math.Pow(Math.Pow(x[i] - x[j], 2) + Math.Pow(y[i] - y[j], 2), 0.5),
                         variogram.nugget,
                         variogram.range,
                         variogram.sill,
                         variogram.A);
                    K[j * n + i] = K[i * n + j];
                }

                K[i * n + i] = variogram.model(0, variogram.nugget, variogram.range, variogram.sill, variogram.A);
            }

            //第五步

            // Inverse penalized Gram matrix projected to target vector
            //反向，，克矩阵投影到目标向量
            List<double> tC = kriging_matrix_diag(sigma2, n);

            List<double> C = kriging_matrix_add(K, tC, n, n);
            List<double> cloneC = new List<double>(C);

            bool is_C_chol = kriging_matrix_chol(ref C, n);
            if(is_C_chol)
            {
                kriging_matrix_chol2inv(ref C, n);
            }
            else
            {
                kriging_matrix_solve(ref cloneC, n);
                C = cloneC;
            }

            List<double> KK = new List<double>(C);
            List<double> MM = kriging_matrix_multiply(C, t, n, n, 1);

            variogram.K = KK;
            variogram.M = MM;



            return variogram;

        }

        // Model prediction
        //模型预测，预测新的坐标点p=(xnew,ynew)的新的值（如高度，温度等）
        double predict(int x, int y, Variogram variogram)
        {

            List<double> k = Rep(variogram.n,0.0);
            for (int i = 0; i < variogram.n; i++)
            {
                k[i] = variogram.model(Math.Pow(Math.Pow(x - variogram.x[i], 2) +
                        Math.Pow(y - variogram.y[i], 2), 0.5),
                    variogram.nugget, variogram.range,
                    variogram.sill, variogram.A);
            }
            return kriging_matrix_multiply(k, variogram.M, 1, variogram.n, 1)[0];
        }

        public class GridResult
        {
            public double[,] A;

            public List<int> xLim;

            public List<int> yLim;

            public int width;
        }

        // Gridded matrices or contour paths
        //网格矩阵或轮廓路径
        //kriging.grid(polygons,variogram,width);
        //使用刚才的variogram对象使polygons描述的地理位置内的格网元素具备不一样的预测值；
        //使用一个边界区域按间距生成grid格网数据数组
        //polygons为区域的坐标数组,可以为多个polygon，variogram为第一步train产生的结果，width为生成grid格网的间距
        public GridResult grid(int[][] polygons,Variogram variogram,int width)
        {
            int n = polygons.Length;
            if(n == 0)
            {
                return null;
            }

            // Boundaries of polygons space
            //多边形空间的边界
            List<int> xlim = new List<int>() { polygons[0][0], polygons[0][0] };
            List<int> ylim = new List<int>() { polygons[0][1], polygons[0][1] };

           
            for (int j = 0; j < n; j++)
            {
                // Vertices
                if (polygons[j][0] < xlim[0])
                    xlim[0] = polygons[j][0];
                if (polygons[j][0] > xlim[1])
                    xlim[1] = polygons[j][0];
                if (polygons[j][1] < ylim[0])
                    ylim[0] = polygons[j][1];
                if (polygons[j][1] > ylim[1])
                    ylim[1] = polygons[j][1];
            }

            int xtarget = 0;
            int ytarget = 0;

            // Alloc for O(n^2) space
            List<int> a = Rep(2, 0);
            List<int> b = Rep(2, 0);

            // Local dimensions
            List<double> lxlim = Rep(2, 0.0);
            List<double> lylim = Rep(2, 0.0);

            int  x = (int)Math.Ceiling((xlim[1] - xlim[0]) / (double)width );
            int y = (int)Math.Ceiling((ylim[1] - ylim[0]) / (double)width );

            double[,] A = new double[101,101];

            //for(int i = 0; i <= x; i++)
            //{
            //    A.Add(Rep(y + 1, 0.0));
            //}

            {

                // Range for polygons[i]
                lxlim[0] = polygons[0][0];
                lxlim[1] = lxlim[0];
                lylim[0] = polygons[0][1];
                lylim[1] = lylim[0];
                for (int j = 1; j < polygons.Length; j++)
                {
                    // Vertices
                    if (polygons[j][0] < lxlim[0])
                        lxlim[0] = polygons[j][0];
                    if (polygons[j][0] > lxlim[1])
                        lxlim[1] = polygons[j][0];
                    if (polygons[j][1] < lylim[0])
                        lylim[0] = polygons[j][1];
                    if (polygons[j][1] > lylim[1])
                        lylim[1] = polygons[j][1];
                }

                // Loop through polygon subspace
                a[0] = (int)Math.Floor(((lxlim[0] - ((lxlim[0] - xlim[0]) % width)) - xlim[0]) / width);
                a[1] = (int)Math.Ceiling(((lxlim[1] - ((lxlim[1] - xlim[1]) % width)) - xlim[0]) / width);
                b[0] = (int)Math.Floor(((lylim[0] - ((lylim[0] - ylim[0]) % width)) - ylim[0]) / width);
                b[1] = (int)Math.Ceiling(((lylim[1] - ((lylim[1] - ylim[1]) % width)) - ylim[0]) / width);

                for (int j = a[0]; j <= a[1]; j++)
                {
                    for (int k = b[0]; k <= b[1]; k++)
                    {

                        xtarget = xlim[0] + j * width;
                        ytarget = ylim[0] + k * width;
                        if (IsPip(polygons, xtarget, ytarget))
                        {
                            A[j,k] = predict(xtarget, ytarget, variogram);
                        }
                    }
                }
            }

            GridResult result = new GridResult();
            result.A = A;
            result.xLim = xlim;
            result.yLim = ylim;
            result.width = width;

            return result;

        }
    }
}
